//
// Created by jg on 2021/4/27.
//

#ifndef MAP_GENERATOR_MAP_GENERATOR_H
#define MAP_GENERATOR_MAP_GENERATOR_H

#include <iostream>
#include <fstream>
#include <eigen3/Eigen/Dense>
#include <memory>
#include <vector>
#include <limits>
#include <algorithm>

#include "BLH2locpos.hpp"
#include "sdf_tools/collision_map.hpp"

using std::vector;
using std::shared_ptr;
using std::min;
using std::max;

namespace MAP_GEN{
    const std::string file_path_
            = std::string("/home/jg/catkin_ws/src/wmr_plan/plan_base/map_generator/map_file/");
    const std::string file_result_
            = std::string("whu_library_square.map");
    const std::string fileA_
            = std::string("square_building.txt");
    const std::string fileB_
            = std::string("friendship_square.txt");
    const std::string fileC_
            = std::string("library_buiding.txt");
    const std::string fileD_
            = std::string("underground_garage.txt");

    std::string double2string(double value){
        std::ostringstream out;
        out.precision(15);
        out << value;
        return out.str();
    }
    /* 地图参数 */
    struct MAPPARAM{
        double resolution_;
        double xmin_, xmax_, ymin_, ymax_;
        double map_margin_;
        Eigen::Vector3d original_;

        void getMapSize(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2){
            xmin_ = min(xmin_, min(p1[0], p2[0]));
            xmax_ = max(xmax_, max(p1[0], p2[0]));
            ymin_ = min(ymin_, min(p1[1], p2[1]));
            ymax_ = max(ymax_, max(p1[1], p2[1]));
        }
    };

    enum STATE{ open = 0, close, init};

    class Edge; //声明
    /* ---------- node of topo graph ---------- */
    class Map_Node{
    public:
        Eigen::Vector3d pos_BLH_;
        Eigen::Vector3d loc_pos_;

        //graph
        vector<shared_ptr<Map_Node>> neighbors_;
        vector<shared_ptr<Edge>>     edges_;

        STATE state_;

        typedef shared_ptr<Map_Node> Ptr;
    public:
        Map_Node(){state_ = STATE::init;}
        Map_Node(Eigen::Vector3d BLH){pos_BLH_ = std::move(BLH);state_ = STATE::init;}

        void setLocpos(Eigen::Vector3d pos){loc_pos_ = std::move(pos);}

        void addNeighbor(Map_Node::Ptr& node){
            this->neighbors_.push_back(node);
        }

    };
    /* ---------- edge of topo graph ---------- */
    class Edge{
    public:
        double length_;
        STATE  state_;
        std::pair<Map_Node::Ptr, Map_Node::Ptr> nodes_;
    public:

        Edge(){state_ = STATE::init;}
        Edge(Map_Node::Ptr& node1, Map_Node::Ptr& node2){
            state_  = STATE::init;
            nodes_  = std::make_pair(node1, node2);
            length_ = (nodes_.first->loc_pos_ - nodes_.second->loc_pos_).norm();
        }

        typedef shared_ptr<Edge> Ptr;
    };
    /* ---------- map generator ---------- */
    class Map_Generator{
    protected:
        BLH2locpos blh2Locpos_;
        MAPPARAM   mapparam_;

        vector<Map_Node::Ptr> graph_;

        sdf_tools::COLLISION_CELL free_cell_;
        sdf_tools::COLLISION_CELL obst_cell_;
        shared_ptr<sdf_tools::CollisionMapGrid> collision_map_;
    public:
        Map_Generator(){
            blh2Locpos_ = BLH2locpos();

            mapparam_.resolution_ = 0.2;
            mapparam_.map_margin_ = 5;
            mapparam_.original_   = blh2Locpos_.getOriginal();

            mapparam_.xmin_ =  std::numeric_limits<float>::max();
            mapparam_.xmax_ = -std::numeric_limits<float>::max();
            mapparam_.ymin_ =  std::numeric_limits<float>::max();
            mapparam_.ymax_ = -std::numeric_limits<float>::max();

            free_cell_ = sdf_tools::COLLISION_CELL(-127);
            obst_cell_ = sdf_tools::COLLISION_CELL( 128);
            collision_map_ = std::make_shared<sdf_tools::CollisionMapGrid>();
        }
        ~Map_Generator(){
            collision_map_ = nullptr;
        }

        void run();

    protected:
        void readFile(std::string file_name, vector<Map_Node::Ptr>& node_vec);

        template<typename T> void vectorMerge(vector<T>& A, vector<T>& B);

        std::string getLineHeader(std::string &line);

        double getFirstNumber(std::string &line);

        void createRoad();

        void createEdge(Map_Node::Ptr& node1, Map_Node::Ptr& node2);

        void createMap();

        void getObstaclePoint(vector<Eigen::Vector3d>& obstacle);

        void writeFile();

        std::string getBLHString(Map_Node::Ptr& node1, Map_Node::Ptr& node2);
    };
}

#endif //MAP_GENERATOR_MAP_GENERATOR_H
